﻿// FLMScanController
// Application logic of FLMScan application
// 
// FLMScan - Featured by FabLab Munich

using System;
using System.Collections.Generic;
using FLMScan.Geometry;
using FLMScan.WorldCoordinateReconstruction.Riedmaier;
using log4net;
using FLMScan.Tools;
using System.Drawing;
using FLMScan.Rotator;
using FLMScan.Parameters;
using FLMScan.Exception;
using FLMScan.Capture;

namespace FLMScan
{
    internal class FLMScanController : IDisposable
    {
        enum ScannerState
        {   eInvalidState
        ,   eIsIdleState
        ,   eIsScanningState
        ,   eIsCalibratingState
        };

        private ILog logger;
        private StateMachine stateMachine;

        private const int DefaultScanResolution = 10;                
        private const double FullCircleDegrees = 360;
        private const int CalibrationStepResolution = 1;
        private const int PlaneCalibrationSteps = 60;

        private bool isDisposed = false;
        private ScannerState state = ScannerState.eInvalidState;
        private FLMScanParameters parameters;

        private IFLMScanView view;
        private ICapture capture;
        private IStepRotator stepRotator;
        private IFreeRotator freeRotator;
        private IReconstruction reconstruction;
        private double currentAngle = 0;
        private int currentStep = 1;
        private int currentCalibrationStep = 0;
        private ObjectData objectData;
        private FileSystem fileSystem;
        private TempFileManager tempFileManager;
        private int currentTempFolderId = 0;
        private FLMScanViewModel viewModel;
        private LaserPlaneCalibrator planeCalibrator;
        private ImageProcessing imageProcessor;
        private List<FlmPoint> ledWorldPositions;

        private int TotalStepCount = DefaultScanResolution;
        private double AnglePerStep;

        private const string LogCaptureInitializationFailed = "Webcam capture initialization failed.";
        private const string LogCaptureInitializationSucceeded = "Webcam capture initialization succeeded.";
        private const string LogRotatorInitializationFailed = "Step Rotator initialization failed.";
        private const string LogRotatorInitializationSucceeded = "Step Rotator initialization succeeded.";
        private const string LogFreeRotatorInitializationFailed = "Free Rotator initialization failed.";                                

        private const string LogCalibrationStepFailed = "Calibration step failed: calibration board LEDs and/or laser line not detected!";

        public FLMScanController(FLMScanParameters parameters, IFLMScanView view, ICapture capture, IStepRotator rotator, IFreeRotator freeRotator, IReconstruction reconstruction, FLMScanViewModel viewModel)
        {
            logger = LogManager.GetLogger(typeof(FLMScanController));
            stateMachine = StateMachine.Instance();

            state = ScannerState.eIsIdleState;
            stateMachine.HandleAction(StateMachine.FlmScanAction.EActionStartup);
                        
            this.parameters = parameters;
            this.view = view;
            this.capture = capture;
            this.stepRotator = rotator;
            this.freeRotator = freeRotator;
            this.reconstruction = reconstruction;
            this.viewModel = viewModel;
            fileSystem = new FileSystem();
            tempFileManager = new TempFileManager(fileSystem, parameters.TemporaryFolder);
            ledWorldPositions = new List<FlmPoint>();
            // TBD: replace this by passing in real LED world coordinates
            InitializeLedWorldPositions();

            CameraSensor cameraSensor = new CameraSensor(parameters.CameraSensorParameters);
            List<double> distortionCoefficients = new List<double>();
            distortionCoefficients.Add(parameters.CameraSensorParameters.DistortionCoefficient1);
            distortionCoefficients.Add(parameters.CameraSensorParameters.DistortionCoefficient2);
            distortionCoefficients.Add(parameters.CameraSensorParameters.DistortionCoefficient3);
            distortionCoefficients.Add(parameters.CameraSensorParameters.DistortionCoefficient4);

            planeCalibrator = new LaserPlaneCalibrator(ledWorldPositions, cameraSensor, distortionCoefficients);
            imageProcessor = new ImageProcessing();

            rotator.SetTotalStepCount(TotalStepCount);
            AnglePerStep = FullCircleDegrees / TotalStepCount;

            rotator.StepDone += HandleRotatorStepDone;
            freeRotator.StepDone += HandleRotatorStepDone;
            view.ViewLoaded += HandleViewLoaded;

            viewModel.OnRequestSave += HandleSaveRequested;
            viewModel.OnRequestStartScan += HandleStartScanRequested;
            viewModel.OnRequestCalibration += HandleCalibratePlaneRequested;
            viewModel.OnRequestStopScan += HandleStopScanRequested;

            stateMachine.OnIdleState += HandleControlActivation;
            stateMachine.OnLaserscanningState += HandleControlDeactivation;
            stateMachine.OnCalibratingState += HandleControlDeactivation;
            stateMachine.OnStoppingState += HandleStopping;

            capture.CaptureDone += HandleCaptureDone;
        }

        private void HandleShowPreferencesRequested(object sender, EventArgs e)
        {
            
        }        

        private void HandleStartScanRequested(object sender, EventArgs e)
        {
            if (CheckDeviceConnectionState() == false)
            {
                viewModel.StatusMessage = Properties.Resources.StatusTextCheckDeviceConnections;
                return;
            }

            int currentScanSteps = 0;

            if (viewModel.DraftScanState)
            {
                currentScanSteps = viewModel.DraftScanSteps;
                viewModel.CurrentScanStep = 0;
                viewModel.TotalScanSteps = viewModel.DraftScanSteps;
            }
            else if (viewModel.FinalScanState)
            {
                currentScanSteps = viewModel.FinalScanSteps;
                viewModel.CurrentScanStep = 0;
                viewModel.TotalScanSteps = viewModel.FinalScanSteps;
            }
            else
            {
                System.Diagnostics.Debug.Assert(false, "Either final or draft scanning must be selected!");
            }

            if (currentScanSteps <= 0)
            {
                System.Diagnostics.Debug.Assert(currentScanSteps > 0, "currentScanSteps must be positive non-null number!");
                return;
            }

            TotalStepCount = currentScanSteps;
            AnglePerStep = FullCircleDegrees / (double)TotalStepCount;

            stepRotator.SetTotalStepCount(TotalStepCount);
            
            this.objectData = new ObjectData();            
            state = ScannerState.eIsScanningState;
            stateMachine.HandleAction(StateMachine.FlmScanAction.EActionStartButton);

            UpdateStatusMessage();

            currentTempFolderId = tempFileManager.StartNewTempFolder();

            NextRotatorStep();
        }        

        private void HandleSaveRequested(object sender, EventArgs e)
        {
            // TBD: implement prompting for save destination
            String destinationPath = System.IO.Path.Combine(parameters.TemporaryFolder, "Saved" + currentTempFolderId);
            tempFileManager.SaveAs(currentTempFolderId, destinationPath);
        }

        private bool CheckDeviceConnectionState()
        {
            bool connectionState = true;
            
            try
            {
                if (!stepRotator.IsConnected)
                {
                    viewModel.ArduinoConnectionState = DeviceState.eUnknown;
                    stepRotator.Initialize();
                    viewModel.ArduinoConnectionState = DeviceState.eConnected;
                    logger.Info(LogRotatorInitializationSucceeded);
                }
            }
            catch (SerialPortException ex)
            {
                connectionState = false;
                viewModel.ArduinoConnectionState = DeviceState.eDisconnected;
                logger.Warn(LogRotatorInitializationFailed + ex);
            }

            try
            {
                if (!capture.IsConnected)
                {
                    viewModel.WebcamConnectionState = DeviceState.eUnknown;
                    capture.Initialize();
                    viewModel.WebcamConnectionState = DeviceState.eConnected;
                    logger.Info(LogCaptureInitializationSucceeded);
                }
            }
            catch (CaptureException ex)
            {
                connectionState = false;
                viewModel.WebcamConnectionState = DeviceState.eDisconnected;
                logger.Warn(LogCaptureInitializationFailed + ex);
            }

            return connectionState;
        }

        private void NextRotatorStep()
        {
            bool isStopping = (stateMachine.CurrentState == StateMachine.FlmScanState.EStateStopping);
            
            // no next rotator step pending or scan stop was requested => serialize object data
            if ( (stepRotator.Step() == false) || isStopping ) 
            {
                objectData.Serialize("vertices");
                tempFileManager.EndTempFolder(currentTempFolderId);

                stepRotator.Reset();
                currentAngle = 0.0;
                currentStep = 1;
                viewModel.CurrentScanStep = 0;

                if( isStopping )
                {
                    state = ScannerState.eIsIdleState;
                    stateMachine.HandleAction(StateMachine.FlmScanAction.EActionStopConfirm);
                }
                else
                {
                    state = ScannerState.eIsIdleState;
                    stateMachine.HandleAction(StateMachine.FlmScanAction.EActionIdle);
                }

                UpdateStatusMessage();           
            }
        }

        private void HandleViewLoaded(object sender, EventArgs e)
        {
            viewModel.StepperPowerState = DeviceState.eUnknown;
            viewModel.LightPowerState = DeviceState.eUnknown;
            viewModel.WebcamConnectionState= capture.IsConnected ? DeviceState.eConnected : DeviceState.eDisconnected;
            viewModel.ArduinoConnectionState = stepRotator.IsConnected ? DeviceState.eConnected : DeviceState.eDisconnected;
            viewModel.DoorContactState = DeviceState.eUnknown;
        }
        
        private void HandleRotatorStepDone(object sender, EventArgs e)
        {
            switch (state)
            {
                case ScannerState.eInvalidState:
                    System.Diagnostics.Debug.Assert(false);
                    break;
                case ScannerState.eIsIdleState:                    
                    break;
                case ScannerState.eIsScanningState:
                    HandleScanRotatorStepDone();
                    break;
                case ScannerState.eIsCalibratingState:
                    HandleCalibrationRotatorStepDone();
                    break;
            }
        }

        private void HandleCaptureDone(object sender, EventArgs e)
        {
            CaptureEventArgs eventArgs = (CaptureEventArgs)e;

            switch (state)
            {
                case ScannerState.eInvalidState:
                    System.Diagnostics.Debug.Assert(false);
                    break;
                case ScannerState.eIsIdleState:
                    System.Diagnostics.Debug.Assert(false);
                    break;
                case ScannerState.eIsScanningState:                    
                    HandleScanCaptureDone(eventArgs.Frame);
                    break;
                case ScannerState.eIsCalibratingState:
                    HandleCalibrationCaptureDone(eventArgs.Frame);
                    break;
            }
        }

        private void HandleScanRotatorStepDone()
        {
            // TBD: add support of passing current angle through EventArgs
            capture.GetFrame();            
        }

        private void HandleScanCaptureDone(CapturedFrame frame)
        {
            if (viewModel.ImageExportState)
            {
                tempFileManager.Serialize(frame.Bitmap, currentTempFolderId, currentStep);
            }

            if (viewModel.GeometryExportState)
            {
                HandleGeometryExport(frame);
            }
            
            view.PictureBoxImage = frame.Bitmap;
            viewModel.CurrentScanStep = currentStep;

            currentAngle += AnglePerStep;
            currentStep++;

            NextRotatorStep();            
        }

        private void HandleGeometryExport(CapturedFrame frame)
        {
            // retrieve 2D laser points from captured frame                     
            List<FlmPoint> imagePlanePoints = imageProcessor.GetLaserPoints(frame);
            frame.MarkFeatures(imagePlanePoints);

            Dictionary<int, SortedList<int, FlmPoint>> pointsByYCoordinate = new Dictionary<int, SortedList<int, FlmPoint>>();
            foreach (FlmPoint imagePoint in imagePlanePoints)
            {
                SortedList<int, FlmPoint> pointList;
                if (pointsByYCoordinate.TryGetValue((int)imagePoint.Y, out pointList))
                {
                    pointList.Add((int)imagePoint.X, new FlmPoint(imagePoint.X, imagePoint.Y, imagePoint.Z));
                }
                else
                {
                    pointList = new SortedList<int, FlmPoint>();
                    pointList.Add((int)imagePoint.X, new FlmPoint(imagePoint.X, imagePoint.Y, imagePoint.Z));
                    pointsByYCoordinate.Add((int)imagePoint.Y, pointList);
                }
            }

            List<FlmPoint> medianPoints = new List<FlmPoint>();

            foreach (int yCoordinate in pointsByYCoordinate.Keys)
            {
                SortedList<int, FlmPoint> points;

                if (pointsByYCoordinate.TryGetValue(yCoordinate, out points))
                {
                    int medianPosition = points.Count / 2;
                    int medianKey = points.Keys[medianPosition];
                    medianPoints.Add(points[medianKey]);
                }
            }

            // convert 2D image plane points to 3D world coordinate points

            double angleInRadians = currentAngle / 360.0 * Math.PI * 2.0;

            List<FlmPoint> worldPoints = new List<FlmPoint>();

            // reconstruct 
            // foreach (FlmPoint imagePoint in imagePlanePoints)
            foreach (FlmPoint imagePoint in medianPoints)
            {
                FlmPoint worldPoint = reconstruction.reconstruct(imagePoint, angleInRadians);
                worldPoints.Add(worldPoint);
            }

            // store 3D world coordinate points
            objectData.AddObjectPoints(currentAngle, worldPoints);
        }

        private void HandleCalibrationRotatorStepDone()
        {
            capture.GetFrame();            
        }

        private void HandleCalibrationCaptureDone(CapturedFrame frame)
        {          
            // retrieve 2D positions of calibration LEDs from captured frame         
            List<FlmPoint> calibrationPoints = imageProcessor.GetCalibrationPoints(frame);
            // retrieve 2D positions of laser line start/end from captured frame
            List<FlmPoint> laserLinePoints = imageProcessor.GetLaserLine(frame);            
                        
            // process calibration data of current step
            if (calibrationPoints.Count == 4) 
            {
                // TBD: make image output conditional and move it to configurable location
                fileSystem.Serialize(frame.Bitmap, @"C:\TEMP\FlmScan\Calibration" + currentCalibrationStep + ".tif");
                frame.MarkFeatures(calibrationPoints);
                fileSystem.Serialize(frame.Bitmap, @"C:\TEMP\FlmScan\CalibrationLED" + currentCalibrationStep + ".tif");

                if (laserLinePoints.Count == 2)
                {
                    frame.MarkFeatures(laserLinePoints);
                    fileSystem.Serialize(frame.Bitmap, @"C:\TEMP\FlmScan\CalibrationLaser" + currentCalibrationStep + ".tif");

                    PlaneCalibratorMeasurement measurement = new PlaneCalibratorMeasurement();
                    measurement.Led1Position2d = calibrationPoints[0];
                    measurement.Led2Position2d = calibrationPoints[1];
                    measurement.Led3Position2d = calibrationPoints[2];
                    measurement.Led4Position2d = calibrationPoints[3];
                    measurement.UpperlaserLinePosition2d = laserLinePoints[0];
                    measurement.LowerLaserLinePosition2d = laserLinePoints[1];

                    planeCalibrator.ProcessMeasurement(measurement);
                }
                else
                {
                    // TBD: make image output conditional and move it to configurable location
                    fileSystem.Serialize(frame.Bitmap, @"C:\TEMP\FlmScan\CalibrationLaserFailed" + currentCalibrationStep + ".tif");
                    logger.Warn(LogCalibrationStepFailed);
                }
            }
            else
            {
                fileSystem.Serialize(frame.Bitmap, @"C:\TEMP\FlmScan\CalibrationLEDFailed" + currentCalibrationStep + ".tif");            
                logger.Warn(LogCalibrationStepFailed);
            }

            view.PictureBoxImage = frame.Bitmap;

            NextPlaneCalibrationStep();
        }

        private void HandleStopScanRequested(object sender, EventArgs e)
        {            
            stateMachine.HandleAction(StateMachine.FlmScanAction.EActionStopButton);
            UpdateStatusMessage();
        }

        private void HandlePreviewRequested(object sender, EventArgs e)
        {
            
        }        

        private void HandleBrowseFileRequested(object sender, EventArgs e)
        {

        }

        private void HandleCalibratePlaneRequested(object sender, EventArgs e)
        {                        
            if (CheckDeviceConnectionState() == false)
            {
                viewModel.StatusMessage = Properties.Resources.StatusTextCheckDeviceConnections;
                return;
            }

            if (!freeRotator.IsConnected)
            {
                try
                {
                    freeRotator.Initialize();
                }
                catch (SerialPortException ex)
                {                    
                    viewModel.ArduinoConnectionState = DeviceState.eDisconnected;
                    logger.Warn(LogRotatorInitializationFailed + ex);
                }
                
            }

            state = ScannerState.eIsCalibratingState;
            stateMachine.HandleAction(StateMachine.FlmScanAction.EActionCalibrateButton);

            currentCalibrationStep = 0;
            UpdateStatusMessage();

            NextPlaneCalibrationStep();            
        }
        
        private void NextPlaneCalibrationStep()
        {
            bool isStopping = (stateMachine.CurrentState == StateMachine.FlmScanState.EStateStopping);

            if ( (currentCalibrationStep < PlaneCalibrationSteps) && !isStopping )
            {
                try
                {
                    ++currentCalibrationStep;
                    freeRotator.StepCounterClockwise(CalibrationStepResolution);
                }
                catch (SerialPortException ex)
                {
                    logger.Warn(LogFreeRotatorInitializationFailed + ex);
                }    
            }
            // last calibration step is reached or calibration stop was triggered
            else
            {                
                state = ScannerState.eIsIdleState;

                if (isStopping)
                {
                    stateMachine.HandleAction(StateMachine.FlmScanAction.EActionStopConfirm);

                    // rotate back to start position
                    freeRotator.StepClockwise(CalibrationStepResolution * (uint)currentCalibrationStep);
                    currentCalibrationStep = 0;
                }
                else
                {
                    currentCalibrationStep = 0;
                    stateMachine.HandleAction(StateMachine.FlmScanAction.EActionIdle);
                    // rotate back to start position
                    freeRotator.StepClockwise(CalibrationStepResolution * PlaneCalibrationSteps);

                    if (planeCalibrator.CanCalculateCalibrationPlane())
                    {
                        FlmPlane laserPlane = planeCalibrator.GetCalibrationPlane();
                        parameters.LaserPlaneParameters.ParameterA = laserPlane.A;
                        parameters.LaserPlaneParameters.ParameterB = laserPlane.B;
                        parameters.LaserPlaneParameters.ParameterC = laserPlane.C;
                        parameters.LaserPlaneParameters.ParameterD = laserPlane.D;
                        parameters.Serialize();
                    }
                }

                UpdateStatusMessage();           
            }
            
        }        

        private void InitializeLedWorldPositions()
        {
            FlmPoint pos1 = new FlmPoint(-5,5,0);
            FlmPoint pos2 = new FlmPoint(5,5,0);
            FlmPoint pos3 = new FlmPoint(5,-5,0);
            FlmPoint pos4 = new FlmPoint(-5,-5,0);

            ledWorldPositions.Add(pos1);
            ledWorldPositions.Add(pos2);
            ledWorldPositions.Add(pos3);
            ledWorldPositions.Add(pos4);
        }

        private void HandleStopping(object sender, EventArgs e)
        {
            // TBD: implement behavior for immediate feedback e.g. different color of stop button
        }        

        private void HandleControlActivation(object sender, EventArgs e)
        {
            SetControlsEnabledState(true);
        }

        private void HandleControlDeactivation(object sender, EventArgs e)
        {
            SetControlsEnabledState(false);
        }

        private void SetControlsEnabledState(bool state)
        {
            viewModel.DraftScanEnabled = state;
            viewModel.FinalScanEnabled = state;
            viewModel.DraftScanStepsEnabled = state;
            viewModel.FinalScanStepsEnabled = state;
        }

        private void UpdateStatusMessage()
        {
            switch (stateMachine.CurrentState)
            {
                case StateMachine.FlmScanState.EStateIdle:
                    viewModel.StatusMessage = Properties.Resources.StatusTextIdle;
                    break;
                case StateMachine.FlmScanState.EStateCalibrating:
                    viewModel.StatusMessage = Properties.Resources.StatusTextCalibrating;
                    break;
                case StateMachine.FlmScanState.EStateLaserscanning:
                    viewModel.StatusMessage = Properties.Resources.StatusTextScanningObject;
                    break;
                case StateMachine.FlmScanState.EStateStopping:
                    viewModel.StatusMessage = Properties.Resources.StatusTextStopping;
                    break;
            }
        }

        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        private void Dispose(bool isDisposing)
        {
            if (isDisposed)
                return;

            if (isDisposing)
            {
                isDisposed = true;

                stepRotator.StepDone -= HandleRotatorStepDone;
                freeRotator.StepDone -= HandleRotatorStepDone;
                view.ViewLoaded -= HandleViewLoaded;
                viewModel.OnRequestSave -= HandleSaveRequested;
                viewModel.OnRequestStartScan -= HandleStartScanRequested;
                viewModel.OnRequestCalibration -= HandleCalibratePlaneRequested;
                stateMachine.OnIdleState -= HandleControlActivation;
                stateMachine.OnLaserscanningState -= HandleControlDeactivation;
                stateMachine.OnCalibratingState -= HandleControlDeactivation;
                capture.CaptureDone -= HandleCaptureDone;

                capture.Dispose();
                tempFileManager.Dispose();                
            }
        }  

    }
}
